#!/usr/bin/env python3
import cv2
import rclpy
from rclpy.node import Node
import cv_bridge
from sensor_msgs.msg import Image
import os


class CameraReader(Node):
    def __init__(self):
        super().__init__("camera_reader")
        self.declare_parameter("device", "undefine")
        self.declare_parameter("width", 1280)
        self.declare_parameter("height", 720)
        self.declare_parameter("fps", 60)
        self.declare_parameter("flip_horizontal", False)
        self.declare_parameter("flip_vertical", False)
        self.declare_parameter("show_image", False)
        device = self.get_parameter("device").get_parameter_value().string_value
        width = self.get_parameter("width").get_parameter_value().integer_value
        height = self.get_parameter("height").get_parameter_value().integer_value
        fps = self.get_parameter("fps").get_parameter_value().integer_value
        self.show = self.get_parameter("show_image").get_parameter_value().bool_value
        self.flip_horizontal = (
            self.get_parameter("flip_horizontal").get_parameter_value().bool_value
        )
        self.flip_vertical = (
            self.get_parameter("flip_vertical").get_parameter_value().bool_value
        )

        if self.flip_horizontal and self.flip_vertical:
            self.flip = -1
        elif self.flip_horizontal:
            self.flip = 1
        elif self.flip_vertical:
            self.flip = 0
        else:
            self.flip = -2
        if device != "undefine":
            self.cap = cv2.VideoCapture(device, cv2.CAP_V4L2)
        else:
            self.cap = cv2.VideoCapture(0, cv2.CAP_V4L2)
        if not self.cap.isOpened():
            self.get_logger().error("Failed to open camera: " + device)
            exit(1)
        self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc("M", "J", "P", "G"))
        # self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc("Y", "U", "Y", "2"))
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
        self.cap.set(cv2.CAP_PROP_FPS, fps)
        self.create_timer(1.0 / fps, self.read_frame_callback)
        self.pub = self.create_publisher(Image, "camera_image", 10)

    def __del__(self):
        self.cap.release()
        cv2.destroyAllWindows()

    def read_frame_callback(self):
        frame = self.read_frame()
        if frame is None:
            return
        if self.show:
            cv2.imshow("frame", frame)
        if cv2.waitKey(1) & 0xFF == ord("c"):
            if not cv2.imwrite(
                os.path.join(os.path.expanduser("~"), "图片/frame.jpg"), frame
            ):
                self.get_logger().error("Failed to save image")
            else:
                self.get_logger().info("Frame captured")
        self.pub.publish(cv_bridge.CvBridge().cv2_to_imgmsg(frame, encoding="bgr8"))

    def read_frame(self):
        ret, frame = self.cap.read()
        if ret:
            if self.flip != -2:
                frame = cv2.flip(frame, self.flip)
            return frame
        else:
            return None

def main(args=None):
    rclpy.init(args=args)
    node = CameraReader()
    node.get_logger().info("Camera Reader node started")
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()
